#include <opencv/cv.h>

#include "astarheuristics.h"

using namespace cv;

int ManhattenDistanceHeuristic::expected(const Path &path, const Point &goal) {
	Node *current = *(path.nodes.end() - 1);
	
	if (!current) {
		return 0;
	}
	
	return abs(current->location.x - goal.x) + abs(current->location.y - goal.y);
}

VoronoiHeuristic::VoronoiHeuristic(const Mat &matrix) :
	m_matrix(matrix)
{
	m_max = 0;
	
	for (int row = 0; row < matrix.size().height; row++) {
		for (int col = 0; col < matrix.size().width; col++) {
			if (m_max < matrix.at<ushort>(row, col)) {
				m_max = matrix.at<ushort>(row, col);
			}
		}
	}
}
	
int VoronoiHeuristic::expected(const Path &path, const Point &goal) {
	int sum = 0;
	
	for (vector<Node *>::const_iterator it = path.nodes.begin();
		it != path.nodes.end(); ++it)
	{
		sum += m_max - m_matrix.at<ushort>((*it)->location.x, (*it)->location.y);
	}
	
	return sum;
}

